% v10: 
% v20: using Nearest Neighbor Matching and RANSAC -> coding structure
% stucks here when the spinal structure is greatly modified!!
% v30: changing the whole critical structure
function y = RSpatternTracker( model,pts,r1,c1,iterNum,thDist,thInlrRatio, mode ) 

  switch mode
      case 'init'
        iterNum = 100;
        sampleNum = 1; % or 5 depending on the mode
                % user input
        % frame396
%         ptSample(1,1:4) = [117 499 508 125];
%         ptSample(2,1:4) = [447 441 71  54];
        % frame 100
%         ptSample(1,1:4) = [220 211 383 413];
%         ptSample(2,1:4) = [193 367 374  209];
        % frame 1
        ptSample(1,1:4) = [270 214 351 418];
        ptSample(2,1:4) = [221 355 393 273];
%         sampleNum = 1;
      case 'next'
        sampleNum = 5;
      otherwise
           error('Wrong RANSAC mode!!');
  end
 
    ptNum = size(pts,2);
    thInlr = round(thInlrRatio*ptNum);
    inlrNum = zeros(1,iterNum);
    Hrec = zeros(3,3,iterNum);
    dist1=zeros(1,iterNum);
    movThres = 50; %100; % 'small move' threshold
    PassThres = 100;
    cntPthres = 0;
    
    % debug var
    cntsmallmov = 0;
    cntprop = 0;

for p = 1:iterNum 
    
    % Get current pose
    v = model{1}(:,:,1); v1 = model{1}(:,:,2); v2 = model{1}(:,:,3); v3 = model{2}(:,:,2);
    A = v(1,:);
    B = v(size(v,1),:);
    C = v1(size(v1,1),:);
    D = v2(1,:);
    E = v3(size(v3,1),:);
    pose = [A' B' C' D' E'];
    
    % Choose samples
    switch mode
        case 'init'
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % 1. fit using 5 random points
            sampleIdx = randIndex(size(r1,1),sampleNum);
            switch mode
                case 'init'
                    ptSample(1,5) = c1(sampleIdx,1);
                    ptSample(2,5) = r1(sampleIdx,1);
                case 'next'
                    ptSample(1,:) = c1(sampleIdx,1);
                    ptSample(2,:) = r1(sampleIdx,1);
                otherwise
                    error('Wrong RANSAC mode!!');
            end
            
          
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        case 'next'
            num = 5;
            Neibor = NearestCorners(pose,[c1';r1'],num);
            samples = [c1';r1'];
            flg = zeros(size(c1,1));
            for i=1:num,
                for j=1:num,
                    for k=1:num,
                        for l=1:num,
                            for m=1:num,
                                ptSample(:,1) = samples(:,Neibor(i,1));
                                flg(Neibor(i,1)) = 1;
                                
                                if ~flg(Neibor(j,2)) 
                                    ptSample(:,2) = samples(:,Neibor(j,2));
                                    flg(Neibor(j,2)) = 1;
                                end
                                
                                if ~flg(Neibor(k,3)) 
                                    ptSample(:,3) = samples(:,Neibor(k,3));
                                    flg(Neibor(k,3)) = 1;
                                end
                                
                                if ~flg(Neibor(l,4)) 
                                    ptSample(:,4) = samples(:,Neibor(l,4));
                                    flg(Neibor(l,4)) = 1;
                                end
                                
                                if ~flg(Neibor(m,5)) 
                                    ptSample(:,5) = samples(:,Neibor(m,5));
                                    flg(Neibor(m,5)) = 1;
                                end
                                
                                % %%%%%% CHECK & RANSAC THE SAMPLE HERE
                                % %%%%%%
                                
                                flg = zeros(size(c1,1));
                            end
                        end
                    end
                end
            end
            
        otherwise
            error('Wrong RANSAC mode!!');
    end 
    
    % Check if this is 'proper' sample set
    if ~checkSample(ptSample),
        cntprop = cntprop + 1;
        continue;
    end;
    
    % Check 'small movement' cond. in 'next' mode
    switch mode   
        case 'init'
        case 'next'
            if norm(ptSample-pose) > movThres, cntsmallmov = cntsmallmov + 1; continue; end;
        otherwise
            error('Wrong RANSAC mode!!');
    end 
    

    
    % Calculate homography model H
    H = HomoMtrxCalc(pose, ptSample);
 
    % Reproject model with H
    modeli = homography(model, H);
    if (iscell(modeli)==0), continue; end;
    
    % 2. count the inliers, if more than thInlr, refit; else iterate 
    for i=1:ptNum 
        val = minDistSearch(pts(:,i),modeli,'analytic');
        dist1(i) = val;
    end 
    inlier1 = find(abs(dist1) < thDist); 
    inlrNum(p) = length(inlier1); 
%     if length(inlier1) < thInlr, continue;  end 
%     if strcmp(mode,'next'),  input('run through already');end
    Hrec(:,:,p)=H;    
    ptSampleR(:,:,p)=ptSample;
%     cntPthres = cntPthres + 1; 
%     if cntPthres >= PassThres,
%         break;
%     end
end 

% 3. choose the coef with the MOST INLINERS (not the minimum distance)
[q,idx] = max(inlrNum)% 
ret=Hrec(:,:,idx); 

modeli = homography(model, ret);

% plotmodel(modeli);

  % model = RSpatternTracker(pts,iterNum,thDist,thInlrRatio);
    
    % Output estimation result
    % plotmodel(model);
cntsmallmov
cntprop
runcase = iterNum - cntprop - cntsmallmov
pc_mov = cntsmallmov/iterNum*100
pc_cntprop = cntprop/iterNum*100

y = modeli;

end


